/*
frigOS
Licensed under GPLv3
*/
#include "kernel.h"
#include "serial.h"
#include "timer.h"
#include "adc.h"
#include "led.h"
#include "button.h"
#include "cm5.h"

#include <stdio.h>
#include <avr/interrupt.h>
#include <avr/io.h>
#include <util/delay.h>

// kernel status variables & arrays
// names should make them self explanatory
volatile TCB thread[MAXIMUM_TASKS];
volatile MCB monitorThread[MAXIMUM_MONITOR_THREADS];
volatile uint8_t stack[STACK_SIZE];
volatile uint8_t currentThread;
volatile uint8_t lastNonIdleThread;
volatile uint8_t totalThreads;
volatile uint8_t tidFree;
volatile uint8_t lastMonitorThread;
volatile uint8_t totalMonitorThreads;


// flag of events fired since the last tick
volatile uint16_t pendingEvents;

Partition partitionList[] = {
    { (void *) &stack[RAIL0 - 1], IDLE_STACK_SIZE, FREE },
    { (void *) &stack[RAIL1 - 1], MINIMUM_STACK_SIZE, FREE },
    { (void *) &stack[RAIL2 - 1], MINIMUM_STACK_SIZE, FREE },
    { (void *) &stack[RAIL3 - 1], MINIMUM_STACK_SIZE, FREE },
    { (void *) &stack[RAIL4 - 1], MINIMUM_STACK_SIZE, FREE },
    { (void *) &stack[RAIL5 - 1], MEDIUM_STACK_SIZE, FREE },
    { (void *) &stack[RAIL6 - 1], MEDIUM_STACK_SIZE, FREE },
    { (void *) &stack[RAIL7 - 1], MAXIMUM_STACK_SIZE, FREE }
};

// initialize the monitor threads array
// no monitor threads are running, so everything just gets zeroed-out
void initMonitorThreads(void)
{
    // no micro threads yet
    totalMonitorThreads = 0;
    lastMonitorThread = 0;

    uint8_t i;
    for(i=0; i<MAXIMUM_MONITOR_THREADS; i++)
    {
        monitorThread[i].execute = NULL;
        monitorThread[i].interval = 0;
        monitorThread[i].count = 0;
        monitorThread[i].tid = NO_MONITOR_THREAD_SCHEDULED;
        monitorThread[i].parentTid = IDLE;
        monitorThread[i].jmpBuf = NULL;
    }
}

// initialize the kernel
// start the idle task (since one thread must always be running)
// and start the stack sanity checker if necessary
void initKernel(void)
{
    pendingEvents = 0x00;
    totalThreads = 0;
    tidFree = 0;
    sinit();

    startThread( (void(*)(void*)) &idle, 0, IDLE_PRIORITY, IDLE_STACK_SIZE );

#ifdef KERNEL_DEBUG

    startThread( (void (*)(void*)) &monitorStack, 0, MINIMUM_PRIORITY, MINIMUM_STACK_SIZE );

#endif

    // Set an invalid task for now
    currentThread = MAXIMUM_TASKS;
}

// print a nice welcome message over USART1
// note that this uses busy-loops instead of interrupt-driven I/O
// since during boot interrupts are disabled
void printBootBanner()
{
    transmitStringUSART1_busyLoop("\n+++++ ");
    transmitStringUSART1_busyLoop(OS_NAME);
    transmitStringUSART1_busyLoop(" v");
    transmitStringUSART1_busyLoop(OS_VERSION);
    transmitStringUSART1_busyLoop(" +++++\n\n");
}

// boot frigOS
// this automagically calls all other necessary initialization functions
// so all main should do is call boot() and get down to starting threads
void boot(void)
{
    // always initialize USART1 so we can send/receive debugging information & boot messages
    initUSART1(USART1_DEFAULT_BAUD, FALSE);

#ifndef QUIET_BOOT
    printBootBanner();
    transmitStringUSART1_busyLoop("initializing hardware...\n");
#endif

    // initialize RS485 (needed for USART0)
#ifndef QUIET_BOOT
    transmitStringUSART1_busyLoop("\t- RS485...");
#endif
    initRS485();
#ifndef QUIET_BOOT
    transmitStringUSART1_busyLoop(" [success]\n");
#endif

    // initialize USART0
#ifndef QUIET_BOOT
    transmitStringUSART1_busyLoop("\t- USART0...");
#endif
    initUSART0();
#ifndef QUIET_BOOT
    transmitStringUSART1_busyLoop(" [success]\n");
#endif

    // initialize LEDs
#ifndef QUIET_BOOT
    transmitStringUSART1_busyLoop("\t- LEDs...");
#endif
    initLEDs();
#ifndef QUIET_BOOT
    transmitStringUSART1_busyLoop(" [success]\n");
#endif

    // initialize ADC
#ifndef DISABLE_ADC
#  ifndef QUIET_BOOT
    transmitStringUSART1_busyLoop("\t- ADC...");
#  endif
    initADC();
#  ifndef QUIET_BOOT
    transmitStringUSART1_busyLoop(" [success]\n");
#  endif
#endif

#ifndef QUIET_BOOT
    transmitStringUSART1_busyLoop("\t- CONTROL TABLE...");
#endif
    initCM5();
#ifndef QUIET_BOOT
    transmitStringUSART1_busyLoop(" [success]\n");
#endif

    // initialize state (may not be necessary later on)
//#ifndef QUIET_BOOT
//    transmitStringUSART1_busyLoop("\t- state...");
//#endif
//    initState();
//#ifndef QUIET_BOOT
//    transmitStringUSART1_busyLoop(" [success]\n");
//#endif

    // initialize hardware buttons
#ifndef DISABLE_BUTTONS
#  ifndef QUIET_BOOT
    transmitStringUSART1_busyLoop("\t- buttons...");
#  endif
    initButtons();
#  ifndef QUIET_BOOT
    transmitStringUSART1_busyLoop(" [success]\n");
#  endif
#endif

#ifndef QUIET_BOOT
    transmitStringUSART1_busyLoop("done\n");

    transmitStringUSART1_busyLoop("starting kernel...");
#endif

    // start up the kernel & related timers
    initKernel();
    initMonitorThreads();
    initTimer3();
    initTimer1();

#ifndef QUIET_BOOT
    transmitStringUSART1_busyLoop(" [success]\n");
#endif
    /*
    cli();
    changeAllLEDs(LED_ON);
    _delay_ms(500);
    changeAllLEDs(LED_OFF);
    _delay_ms(500);
    changeAllLEDs(LED_ON);
    _delay_ms(500);
    changeAllLEDs(LED_OFF);
    */
}


// start a new thread
// returns TRUE if successful, otherwise returns FALSE
uint8_t startThread(void (*functionPtr)(void *arg), void *arg, uint8_t priority, uint16_t stackSize)
{
    uint8_t result = FALSE;
    uint8_t id;

    cli();
    id = talloc();

    if (id != MAXIMUM_TASKS)
    {
        thread[id].tid = id;
        thread[id].state = READY;
        thread[id].priority = priority;
        thread[id].sleep = 0;
        thread[id].niceness = 125;
        thread[id].monitorLevel = 0;

        if ( ( thread[id].stackPtr  = salloc(stackSize) ) != NULL )
        {
            // create illusion new task was just interrupted
            uint16_t stackPtr = SP;
            SP = (uint16_t) thread[id].stackPtr;

            asm volatile(
                "push %0\n\t"	// PC - low byte (stop function)
                "push %1\n\t"	// PC - high byte
                "push %2\n\t"	// PC - low byte (start function)
                "push %3\n\t"	// PC - high byte
                "push __zero_reg__\n\t"	// __zero_reg__
                "push __tmp_reg__\n\t"	// __tmp_reg__
                "in __tmp_reg__,__SREG__\n\t"
                "push __tmp_reg__\n\t"	// SREG
                "in __tmp_reg__,0x3b\n\t"
                "push __tmp_reg__\n\t"	// RAMPZ
                "push r2\n\t"
                "push r3\n\t"
                "push r4\n\t"
                "push r5\n\t"
                "push r6\n\t"
                "push r7\n\t"
                "push r8\n\t"
                "push r9\n\t"
                "push r10\n\t"
                "push r11\n\t"
                "push r12\n\t"
                "push r13\n\t"
                "push r14\n\t"
                "push r15\n\t"
                "push r16\n\t"
                "push r17\n\t"
                "push r18\n\t"
                "push r19\n\t"
                "push r20\n\t"
                "push r21\n\t"
                "push r22\n\t"
                "push r23\n\t"
                "push %4\n\t"	// r24 start function arg - low byte (avr-gcc call convention)
                "push %5\n\t"	// r25 start function arg - high byte
                "push r26\n\t"
                "push r27\n\t"
                "push r28\n\t"
                "push r29\n\t"
                "push r30\n\t"
                "push r31\n\t"
    :
    :
                "r" ( (uint8_t) ( (uint16_t) &stopThread ) ),		// PC - low byte (stop function)
                "r" ( (uint8_t)( ( (uint16_t) &stopThread ) >> 8 ) ), 	// PC - high byte
                "r" ( (uint8_t) ( (uint16_t) functionPtr ) ),		// PC - low byte (start function)
                "r" ( (uint8_t)( ( (uint16_t) functionPtr ) >> 8 ) ),// PC - high byte
                "r" ( (uint8_t) ( (uint16_t) arg ) ),			// start function arg - low byte
                "r" ( (uint8_t)( ( (uint16_t) arg ) >> 8 ) )		// start function arg - high byte
            );

            thread[id].stackPtr  = (void *) SP;

            SP = stackPtr;
            result = TRUE;
        }
        else   //if we couldn't allocated the stack then free the id
        {
            tfree(id);
        }
    }
    sei();
    return result;
} // startThread

// stop a thread
// this does NOT free up the resources (e.g. semaphores) held by the thread
// it only cleans up the stack space and allows it to be recycled
void stopThread(void)
{
#ifdef KERNEL_DEBUG
    lockSemaphore(&usart1_tx_lock);
    transmitStringUSART1("stopTask\n");
    unlockSemaphore(&usart1_tx_lock);
#endif

    cli();
    thread[currentThread].state = STOPPED;
    tfree(currentThread);
    sfree(thread[currentThread].stackPtr);
    sei();

    while(1)
        asm volatile("nop"::);

} // stopTask

// put the current thread to sleep for a given number of ticks
void sleep(uint16_t ticks)
{
    thread[currentThread].sleep = ticks1 + ticks;
    thread[currentThread].state = SLEEPING;

    while (thread[currentThread].sleep != 0)
        asm volatile("nop"::);
} // sleep


// like sleep this will pause the task, but instead of waiting x ticks we
// wait until a specified interrupt fires (e.g. USART0_Rx, a2x complete, etc...)
void wait(Event event)
{
    cli();
    thread[currentThread].waitingFor = event;   // clear any old results
    thread[currentThread].state = WAITING;      // put the task to sleep
    sei();

    while(thread[currentThread].state == WAITING)
        asm volatile("nop"::);
}

// wait for an event, but wakeup prematurely if we wait too long
void waitTimeout(Event event, uint16_t ticks)
{
    cli();
    thread[currentThread].waitingFor = event;
    thread[currentThread].state = WAIT_TIMEOUT;
    thread[currentThread].sleep = ticks1 + ticks;
    sei();

    while(thread[currentThread].state == WAIT_TIMEOUT)
        asm volatile("nop"::);
}

// called by ISRs to wake tasks up from its waiting state
// all tasks that were waiting for the specified trigger are reset to READY
// and their waitingFor bit-fields are cleared except for the one that triggered the event
void fireEvent(Event event)
{
    pendingEvents |= event;
}

// blocking semaphore with priority and niceness
void initSemaphore(volatile Semaphore *semaphore, uint8_t value)
{
    semaphore->value = -value;               // internally positive values indicate that the semaphore is locked
    semaphore->lockedBy = thread[IDLE].tid;  // initially semaphore is owned by the idle task
    semaphore->queueLength = 0;              // nobody's waiting in line yet
} // initSemaphore


// note: do not call lockSemaphore within an interrupt context
// lock a semaphore if it is unlocked or we own the lock
// otherwise queue the current thread and wait
void lockSemaphore(volatile Semaphore *semaphore)
{
    cli();

    if (semaphore->value >= 0)   // the semaphore is currently locked
    {

        if(semaphore->lockedBy == thread[currentThread].tid)
        {
            semaphore->value++;
        }
        else
        {
            // insert in order of priority and niceness
            uint8_t x;

            for (x = 0; x < semaphore->queueLength; x++)
            {

                if ( thread[currentThread].priority > thread[ semaphore->waitList[x] ].priority &&
                        thread[currentThread].niceness < thread[ semaphore->waitList[x] ].niceness )
                {

                    uint8_t temp_task = semaphore->waitList[x];
                    uint8_t i;

                    for (i = x + 1; i < semaphore->queueLength; i++)
                    {

                        uint8_t swap = semaphore->waitList[i];
                        semaphore->waitList[i] = temp_task;
                        temp_task = swap;

                    }

                    semaphore->waitList[i] = temp_task;

                    break;

                }

            }

            semaphore->waitList[x] = currentThread;
            thread[currentThread].state = BLOCKED;

            semaphore->queueLength++;
        }
    }
    else // the semaphore is currently available
    {
        semaphore->value++;
        semaphore->lockedBy = thread[currentThread].tid;
    }

    sei();

    while (thread[currentThread].state == BLOCKED)
    {
        asm volatile("nop"::);
    }
} // lockSemaphore

// unlock a semaphore and dequeue a waiting thread if necessary
void unlockSemaphore(volatile Semaphore *semaphore)
{

    cli();

    semaphore->value--;

    if (semaphore->value < 0 && semaphore->queueLength>0)
    {
        // we just unlocked the semaphore and there is 1+ thread waiting to lock it

        uint8_t x;
        thread[semaphore->waitList[0]].state = READY;
        semaphore->lockedBy = thread[semaphore->waitList[0]].tid;
        semaphore->queueLength--;

        for(x=1; x<=semaphore->queueLength; x++)
            semaphore->waitList[x-1] = semaphore->waitList[x];
    }
    else if (semaphore->value < 0)
    {
        // the semaphore is unlocked and there is no queue of other threads wanting it
        semaphore->lockedBy = IDLE;
    }

    sei();
} // unlockSemaphore

// cede control of a semaphore completely; we unlock it completely
// and pass it off to the next thread that wants it
// this should only ever be used with asynchronous while loops that use
// a semaphore in their loop that may not be unlocked before the
// long-jump is executed
void cedeSemaphore(volatile Semaphore* semaphore)
{
    while(semaphore->lockedBy == thread[currentThread].tid)
        unlockSemaphore(semaphore);
}

// returns whether or not we *could* lock the semaphore if we wanted to
// i.e. is the semaphore either available or owned by us already?
// FALSE - someone else owns the lock
// TRUE - we can (re-)lock
BOOL tryLockSemaphore(volatile Semaphore *semaphore)
{
    uint8_t result = FALSE;

    if (semaphore->value < 0 || semaphore->lockedBy == thread[IDLE].tid || semaphore->lockedBy == thread[currentThread].tid)
    {
        result = TRUE;
    }

    return result;
} // tryLockSemaphore

// initialize the stack and set up the rails
void sinit(void)
{
    stack[RAIL0] = SCHECK;
    stack[RAIL1] = SCHECK;
    stack[RAIL2] = SCHECK;
    stack[RAIL3] = SCHECK;
    stack[RAIL4] = SCHECK;
    stack[RAIL5] = SCHECK;
    stack[RAIL6] = SCHECK;
    stack[RAIL7] = SCHECK;
    stack[RAIL8] = SCHECK;
} // sinit

// allocate a block of stack space using a best fit allocation policy
// returns a pointer to the allocated stack, or NULL if no stack partitions were available
void * salloc(uint16_t stackSize)
{
    void *result = NULL;
    uint8_t x;
    uint8_t bestFit = MAXIMUM_TASKS;
    uint16_t smallestDifference = 0xffff;

    // loop through all the available paritions and check them
    // we can stop if we ever find a perfect fit
    for (x = 0; x < MAXIMUM_TASKS && smallestDifference>0; x++)
    {
        if (stackSize <= partitionList[x].stackSize && partitionList[x].status == FREE)
        {
            // this is a possible candidate; see if it's better than the best we've found so far
            if (partitionList[x].stackSize - stackSize < smallestDifference)
            {
                smallestDifference = partitionList[x].stackSize - stackSize;
                bestFit = x;
            }
        }
    }

    // see if we found anything that fit
    if (bestFit < MAXIMUM_TASKS)
    {
        partitionList[bestFit].status = USED;
        result = partitionList[bestFit].stackPtr;
    }

    return result;
} // salloc

// 1 byte rails between stack partitions containing sentinel value
// if any rail value is not equal to the sentinel value, stack overflow or underflow has occured
// TRUE = success, FALSE = fail
BOOL scheck(void)
{

    uint8_t result = TRUE;

    if (	stack[RAIL0] != SCHECK ||
            stack[RAIL1] != SCHECK ||
            stack[RAIL2] != SCHECK ||
            stack[RAIL3] != SCHECK ||
            stack[RAIL4] != SCHECK ||
            stack[RAIL5] != SCHECK ||
            stack[RAIL6] != SCHECK ||
            stack[RAIL7] != SCHECK ||
            stack[RAIL8] != SCHECK )
    {
        result = FALSE;
    }

    return result;
} // scheck

// free a block of stack space used by a thread
void sfree(void *stackPtr)
{
    uint8_t x;

    for (x = 0; x < MAXIMUM_TASKS; x++)
    {
        if ( stackPtr <= partitionList[x].stackPtr && stackPtr >= (partitionList[x].stackPtr - partitionList[x].stackSize) )
        {
            partitionList[x].status = FREE;
        }
    }
} // sfree

// allocate a TCB for a new thread
uint8_t talloc(void)
{

    uint8_t result = MAXIMUM_TASKS;
    uint8_t x;
    if (totalThreads < MAXIMUM_TASKS)
    {

        for (x = 0; x < 8; x++)
        {
            if ( ( tidFree & (1 << x) ) == 0 )
            {
                tidFree |= (1 << x);
                totalThreads++;
                result = x;
                break;
            }
        }
    }

    return result;
} // talloc

// free a TCB for reuse
void tfree(uint8_t tid)
{
    tidFree &=  ( ~ (1 << tid) );
    totalThreads--;
} // tfree

// the IDLE task
// at least one thread must always be running, and this is the default
// if all *real* threads are waiting, blocked, or stopped
void idle(void *arg)
{
    uint8_t i;

    for(;;)
    {
        changeLED( MANAGE_LED, LED_TOGGLE );
        for( i = 0; i < 100; i++ )
        {
            _delay_ms(100);
        }
    }
} // idle

// priority scheduling with niceness value to prevent starvation
// tasks of equal priority are round-robin scheduled
void schedule(void)
{
    uint8_t i;  // raw loop counter
    uint8_t x;  // the current task the loop considers; (last non-idle task # + i) MOD MAX_TASKS

    // because interrupts are enabled we need a local copy of events we're actually firing
    uint8_t firedEvents = pendingEvents;

    if ( ( currentThread < MAXIMUM_TASKS) && (thread[currentThread].state == RUNNING) )
    {
        thread[currentThread].state = READY;
    }
    else
    {
        currentThread = IDLE;
    }

    // walk through the threads and figured out what we should run next
    for (i = 0; i < MAXIMUM_TASKS; i++)
    {
        // start counting at the task just past the last one executed
        // this will ensure some degree of round-robin scheduling
        x = (lastNonIdleThread+i)%MAXIMUM_TASKS;

        if ( (tidFree & (1 << x)) != 0 )
        {
            if ((thread[x].state == WAITING || thread[x].state == WAIT_TIMEOUT) && (thread[x].waitingFor & firedEvents))
            {
                // wake up threads that were waiting for events
                thread[x].state = READY;
                thread[x].waitingFor = thread[x].waitingFor & firedEvents;
            }
            else if ((thread[x].state == SLEEPING || thread[x].state==WAIT_TIMEOUT) && (thread[x].sleep == ticks1))
            {
                // handle timeouts while waiting for events
                thread[x].waitingFor = 0;   // clear all events for WAIT_TIMEOUT
                thread[x].sleep = 0;
                thread[x].state = READY;
            }

            if (thread[x].state == READY)
            {

                if (thread[x].priority >= thread[currentThread].priority)
                {
                    currentThread = x;
                }
                else if (thread[x].priority < thread[currentThread].priority)
                {
                    if (thread[x].niceness < thread[currentThread].niceness)
                    {

                        thread[currentThread].niceness--;
                        currentThread = x;
                    }
                    else
                    {
                        thread[x].niceness--;
                    }

                }

            }
        }
    }

    // if we're not idling then update the last non-idle task ID
    if(currentThread!=IDLE)
        lastNonIdleThread = currentThread;

    thread[currentThread].niceness++;
    thread[currentThread].state = RUNNING;

    // clear the pending events
    // additional events may have been fired
    pendingEvents = pendingEvents ^ firedEvents;

#ifdef KERNEL_DEBUG
    // transmit T + the ID of the task we scheduled (Ti for idle)
    transmitStringUSART1("T");
    char c;
    switch(currentThread)
    {
    case IDLE:
        transmitStringUSART1("i");
        break;
    default:
        c = '0'+currentThread;
        transmitBufferUSART1(&c,1);
        break;
    }
#endif
} // schedule


// set up a monitor thread to run if possible
// interval is the number of ticks that must elapse before the thread will execute
// returns the TID of the new monitor thread
uint8_t startMonitorThread( uint8_t (*functionPtr)(void), uint16_t interval )
{
    cli();
    // first off determine the first available index into which we can
    // insert a monitor thread
    uint8_t index = 0;
    for (index = 0; index<MAXIMUM_MONITOR_THREADS && monitorThread[index].tid!=NO_MONITOR_THREAD_SCHEDULED; index++);

    // if we couldn't start the thread return 0xff
    if(index >= MAXIMUM_MONITOR_THREADS)
    {
        sei();
        return NO_MONITOR_THREAD_SCHEDULED;
    }

    // initialize the new monitor thread
    monitorThread[index].execute = functionPtr;
    monitorThread[index].interval = interval;
    monitorThread[index].count = 0;
    monitorThread[index].tid = index;
    monitorThread[index].parentTid = thread[currentThread].tid;
    monitorThread[index].jmpBuf = NULL;

    monitorThread[index].level = thread[monitorThread[index].parentTid].monitorLevel;
    thread[monitorThread[index].parentTid].monitorLevel++;

    totalMonitorThreads++;

    sei();
    return monitorThread[index].tid;
}

// start a monitor thread slaved to the current thread that is set up to long-jump
// back to a particular point in time if it returns false
uint8_t startAsyncWhileThread( uint8_t (*functionPtr)(void), jmp_buf* jmpBuf )
{
    cli();

    // find out where we can insert a new monitor thread
    uint8_t index = 0;
    for (index = 0; index<MAXIMUM_MONITOR_THREADS && monitorThread[index].tid!=NO_MONITOR_THREAD_SCHEDULED; index++);

    if(index >= MAXIMUM_MONITOR_THREADS)
    {
        sei();
        return NO_MONITOR_THREAD_SCHEDULED;
    }

    monitorThread[index].execute = functionPtr;
    monitorThread[index].interval = 0;
    monitorThread[index].count = 0;
    monitorThread[index].tid = index;
    monitorThread[index].parentTid = thread[currentThread].tid;
    monitorThread[index].jmpBuf = jmpBuf;

    monitorThread[index].level = thread[monitorThread[index].parentTid].monitorLevel;
    thread[monitorThread[index].parentTid].monitorLevel++;

    totalMonitorThreads++;

    sei();
    return monitorThread[index].tid;
}

// stop a monitor thread specified by an ID
void stopMonitorThread( uint8_t id)
{
    uint8_t i;
    for(i=0; i<MAXIMUM_MONITOR_THREADS && monitorThread[i].tid!=id; i++);
    if(i == MAXIMUM_MONITOR_THREADS)
    {
        return;
    }


    uint8_t parentId = monitorThread[i].parentTid;

    thread[parentId].monitorLevel = monitorThread[i].level;

    // kill all monitor threads whose level is deeper than this one
    for (i=0; i<MAXIMUM_MONITOR_THREADS; i++)
    {
        if(monitorThread[i].parentTid == parentId && monitorThread[i].level >= thread[parentId].monitorLevel)
        {
            monitorThread[i].execute=NULL;
            monitorThread[i].tid = NO_MONITOR_THREAD_SCHEDULED;
            monitorThread[i].interval = 0;
            monitorThread[i].count = 0;
            monitorThread[i].parentTid = IDLE;
            //monitorThread[i].jmp_buf = NULL;

            totalMonitorThreads--;
        }
    }
}

// determine the next monitor thread we need to run and return a pointer to it
volatile MCB* scheduleMonitorThreads()
{
    volatile MCB* ret = NULL;
    uint8_t i, x;
    for(i=1; i<=MAXIMUM_MONITOR_THREADS; i++)
    {
        x = (i+lastMonitorThread)% MAXIMUM_MONITOR_THREADS;

        monitorThread[x].count++;

        // if we can schedule this thread then return it
        if(monitorThread[x].count >= monitorThread[x].interval && monitorThread[x].tid != NO_MONITOR_THREAD_SCHEDULED && ret == NULL)
        {

            // we might be able to schedule this monitor if it does not require long-jumping, or if the parent thread is scheduled
            if (monitorThread[x].jmpBuf==NULL || monitorThread[x].parentTid == thread[currentThread].tid)
            {
                lastMonitorThread = x;
                monitorThread[x].count = 0;   // reset the count

                ret = &(monitorThread[x]);
            }
        }
    }

    return ret;
}

// kernel panic; something catastrophic happened
// and we need to kill EVERYTHING and wait for a reset
// flash all the LEDs continuously until a hard reset
void panic(uint8_t errCode)
{
    cli();          // ensure interrupts are turned off!
    setRxUSART0();  // make sure we can't send messages to the servos that might damage them

    powerLEDOn();
    playLEDOn();
    auxLEDOn();
    rxdLEDOn();
    txdLEDOn();
    programLEDOn();
    manageLEDOn();

    for(;;)
    {
        transmitByteUSART1(errCode);    // send the error code to the host PC

        changeLED(POWER_LED, LED_TOGGLE);
        changeLED(PLAY_LED, LED_TOGGLE);
        changeLED(AUX_LED, LED_TOGGLE);
        changeLED(RXD_LED, LED_TOGGLE);
        changeLED(TXD_LED, LED_TOGGLE);
        changeLED(PROGRAM_LED, LED_TOGGLE);
        changeLED(MANAGE_LED, LED_TOGGLE);

        _delay_ms(250);
    }
}

// monitor the memory and panic of anything ever goes wrong
void monitorStack(void *arg)
{
    uint8_t memoryOK;
    for(;;)
    {
        memoryOK = scheck();
        if(!memoryOK)
            panic(ERR_STACK_OVERFLOW);

        sleep(100);
    }
} // memoryCheck

#ifdef KERNEL_DEBUG

// flash the AUX led
void testTask0(void *arg)
{
    for(;;)
    {
        changeLED(AUX_LED, LED_TOGGLE);
        sleep((uint8_t)arg);
    }
} // testTask0

// flash the PROGRAM led, but with a recursively-locked semaphore
void testTask1(void *arg)
{
    volatile Semaphore sem1;
    initSemaphore(&sem1, 1);

    for(;;)
    {
        lockSemaphore(&sem1);
        lockSemaphore(&sem1);
        changeLED(PROGRAM_LED, LED_TOGGLE);
        unlockSemaphore(&sem1);
        unlockSemaphore(&sem1);
        sleep((uint8_t)arg);
    }
} // testTask1

// flash the PLAY led
void testTask2(void *arg)
{
    for(;;)
    {
        changeLED(PLAY_LED, LED_TOGGLE);
        sleep((uint8_t)arg);
    }
} // testTask2

// toggle the POWER led
uint8_t testMonitor0()
{
    changeLED(POWER_LED, LED_TOGGLE);

    return PORT_LED & POWER_LED;
}

// toggle the PLAY led
uint8_t testMonitor1()
{
    changeLED(PLAY_LED, LED_TOGGLE);

    return PORT_LED & PLAY_LED;
}

#endif
